#include "SerialControl_c.h"
#include "SerialControl.h"
#include <string>
#include <vector>
#include <cstring>

extern "C" {

void* serial_control_init(const char* port, int baudrate, int timeout) {
    try {
        // Create a new SerialControl instance
        SerialControl* sc = new SerialControl(port, baudrate, timeout);
        return static_cast<void*>(sc);
    } catch (const std::exception& e) {
        fprintf(stderr, "Error initializing SerialControl: %s\n", e.what());
        return NULL;
    }
}

void serial_control_set_move_time(void* handle, int move_time_ms) {
    if (!handle) return;
    
    SerialControl* sc = static_cast<SerialControl*>(handle);
    
    // Use the new direct method to set move time
    sc->setDirectMoveTime(move_time_ms);
}

int serial_control_send_joint_command(void* handle, const double* rads, int count, const char* machine) {
    if (!handle || !rads || !machine) return 0;
    
    try {
        SerialControl* sc = static_cast<SerialControl*>(handle);
        
        // Convert C array to std::vector
        std::vector<double> radsVec(rads, rads + count);
        
        // Send the command
        sc->sendCommand(radsVec, machine);
        return 1;
    } catch (const std::exception& e) {
        fprintf(stderr, "Error sending joint command: %s\n", e.what());
        return 0;
    }
}

int serial_control_get_rads_data(void* handle, const char* machine, double* rads_out, int count) {
    if (!handle || !machine || !rads_out) return -1;
    
    try {
        SerialControl* sc = static_cast<SerialControl*>(handle);
        
        // Get the rads data
        std::vector<double> result = sc->getRadsData(machine);
        
        // Copy to output array, but don't exceed count
        int returnCount = (result.size() <= static_cast<size_t>(count)) ? 
                           result.size() : count;
        
        for (int i = 0; i < returnCount; i++) {
            rads_out[i] = result[i];
        }
        
        return returnCount;
    } catch (const std::exception& e) {
        fprintf(stderr, "Error getting rads data: %s\n", e.what());
        return -1;
    }
}

void serial_control_close(void* handle) {
    if (!handle) return;
    
    SerialControl* sc = static_cast<SerialControl*>(handle);
    sc->closeSerial();
    delete sc;
}

void serial_control_release_force(void* handle, const char* machine) {
    if (!handle || !machine) return;

    SerialControl* sc = static_cast<SerialControl*>(handle);
    sc->releaseForce(machine);
}

void serial_control_recover_force(void* handle, const char* machine) {
    if (!handle || !machine) return;

    SerialControl* sc = static_cast<SerialControl*>(handle);
    sc->recoverForce(machine);
}

} // extern "C"